/**
 * DCMotor
 *
 * Authors: Bruna and Bruno
 * Date: Jun, 22 2012
 **/
 
#include <Arduino.h>
#include <DCMotor.h>

#define MAX_SPEED 255

#define PIN_MOTOR_LEFT_0 5
#define PIN_MOTOR_LEFT_1 6

#define PIN_MOTOR_RIGHT_0 10
#define PIN_MOTOR_RIGHT_1 11

#define ENCODER_TICKS_PER_TURN 9

#define SETPOINT 2
#define SAMPLING_RATE 7
#define SAMPLINGS 1000 / SAMPLING_RATE

int DCMotor::count_left_ticks = 0;
int DCMotor::count_right_ticks = 0;

DCMotor::DCMotor(int _kp, float _kd, float _ki) {
    left_speed = MAX_SPEED;
    right_speed = MAX_SPEED;
    
    kp = _kp;
    kd = _kd;
    ki = _ki;
}
   
void DCMotor::begin() {
    last_check_inst = millis();
    Ierrorl = 0;
    Ierrorr = 0;
    
    attachInterrupt(0, inc_left_ticks, CHANGE);
    attachInterrupt(1, inc_right_ticks, CHANGE);
}

void DCMotor::setMotorVelocity() {
    analogWrite(PIN_MOTOR_LEFT_0, left_speed);
    analogWrite(PIN_MOTOR_LEFT_1, 0);

    analogWrite(PIN_MOTOR_RIGHT_0, right_speed); 
    analogWrite(PIN_MOTOR_RIGHT_1, 0);
}

void DCMotor::forward() {
    long check_inst = millis();
    if (check_inst - last_check_inst > SAMPLINGS) {
        doPID();
        setMotorVelocity();
        last_check_inst = check_inst;
    }
}

void DCMotor::stop() {
    left_speed = 0;
    right_speed = 0;
    
    setMotorVelocity();
}

void DCMotor::left() {
    int old_left_speed = left_speed;
    int old_right_speed = right_speed;
    int old_ticks = count_left_ticks;
   
    left_speed = 0;
    right_speed = MAX_SPEED;
    setMotorVelocity();

    count_right_ticks = 0;
    while (count_right_ticks < ENCODER_TICKS_PER_TURN) {
        Serial.println("TURNING_LEFT");
    }
    
    count_right_ticks = old_ticks;
    left_speed = old_left_speed;
    right_speed = old_right_speed;
    setMotorVelocity();
}

void DCMotor::right() {
    int old_left_speed = left_speed;
    int old_right_speed = right_speed;
    int old_ticks = count_left_ticks;
   
    left_speed = MAX_SPEED;
    right_speed = 0;
    setMotorVelocity();

    count_left_ticks = 0;
    while (count_left_ticks < ENCODER_TICKS_PER_TURN) {
        Serial.println("TURNING_RIGHT");
    }
    
    count_left_ticks = old_ticks;
    left_speed = old_left_speed;
    right_speed = old_right_speed;
    setMotorVelocity();
}

void DCMotor::doPID() { 
  int errorl; //error from left encoder
  int errorr; //error from right encoder
  int derrorl; //derivative error left
  int derrorr; //derivative error right
  int cl;
  int cr; 
  
  errorl = SETPOINT - count_left_ticks; //calculate error values
  errorr = SETPOINT - count_right_ticks; 
  
  count_left_ticks = 0; //reset encoder counts ready for next sample
  count_right_ticks = 0;
  
  derrorl = (errorl - preverrorl); 
  derrorr = (errorr - preverrorr);
  
  cl = ((kp*errorl) + (kd*derrorl) + (ki*Ierrorl)); //PID equations
  cr = ((kp*errorr) + (kd*derrorr) + (ki*Ierrorr));
  
  if((left_speed + cl) > MAX_SPEED) //prevent speed from overrunning MAX_SPEED
    left_speed = MAX_SPEED;
  else
    left_speed = left_speed + cl; //use output from PID equations to alter motor speeds 
    
  if((right_speed + cr) > MAX_SPEED) //where speed is PWM output values
    right_speed = MAX_SPEED;
  else
    right_speed = right_speed + cr;
  
  preverrorl = errorl; //set previous error to current error
  preverrorr = errorr;
  
  Ierrorl = Ierrorl + errorl; //add current error to integral error
  Ierrorr = Ierrorr + errorr;
}

void DCMotor::inc_left_ticks() {
    DCMotor::count_left_ticks++;
}

void DCMotor::inc_right_ticks() {
    DCMotor::count_right_ticks++;
}
